#ifndef WIN32
#include "socket_can.h"
#include <errno.h>
#include <fcntl.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/uio.h>
#include <unistd.h>
#include "terminal.h"
#include "socket_can.h"

RawCAN::RawCAN(unsigned short _id, unsigned char* _data, unsigned short _dlc,
               unsigned int _timestamp)
    : id(_id), dlc(_dlc), timestamp(_timestamp) {
  memcpy(data, _data, dlc);
}

struct ::can_frame RawCAN::can_frame() const {
  struct ::can_frame frame;
  memset(&frame, 0, sizeof(frame));
  frame.can_id = id;
  frame.can_dlc = dlc;
  memcpy(frame.data, data, frame.can_dlc);
  return frame;
}

struct ::canfd_frame RawCAN::canfd_frame() const {
  struct ::canfd_frame frame;
  memset(&frame, 0, sizeof(frame));
  frame.can_id = id;
  frame.len = can_fd_dlc2len(can_fd_len2dlc(dlc));
  memcpy(frame.data, data, frame.len);
  return frame;
}

std::string RawCAN::ToString() const {
  std::string str(size_t(CL_LONGCFSZ), '\0');
  auto frame = canfd_frame();
  sprint_long_canframe(&str[0], &frame, 0,
                       (dlc <= CAN_MAX_DLEN) ? CAN_MAX_DLEN : CANFD_MAX_DLEN);
  if (id & CAN_ERR_FLAG) {
    std::string err(size_t(CL_LONGCFSZ), '\0');
    snprintf_can_error_frame(&err[0], err.size(), &frame, ", ");
    str = str + err;
  }
  return str;
}

SocketCAN::SocketCAN() : can_name_("can0") { socket_ = 0; }

SocketCAN::SocketCAN(const std::string& dev_name, bool absolute_timestamp)
    : can_name_(dev_name), absolute_timestamp_(absolute_timestamp) {
  socket_ = 0;

  char channel = can_name_.back();
  if ((channel >= '0') && (channel <= '9')) {
    channel_id_ = channel - '0' + 1;
  } else {
    channel_id_ = uint8_t(channel);
  }
}

SocketCAN::~SocketCAN() { close(socket_); }

void SocketCAN::SetTransciverOff() {
  int ret = 0;
  int fd = open(tx2_gpio_.c_str(), O_RDWR);
  if (fd < 0) {
    printf("failed to open calmcargpio\n");
    return;
  }

  if (can_name_ == "can0") {
    ret = ioctl(fd, 8, 1);
  }
  if (can_name_ == "can1") {
    ret = ioctl(fd, 9, 1);
  }
  if (can_name_ == "can2") {
    ret = ioctl(fd, 10, 1);
  }

  if (ret < 0) {
    printf("%s: failed to set can transciver off\n", can_name_.c_str());
  }
  close(fd);
}

void SocketCAN::SetTransciverOn() {
  if (access(tx2_gpio_.c_str(), F_OK) != 0) {
    printf("failed to access calmcargpio\n");
    return;
  }

  sleep(10);
  int fd = open(tx2_gpio_.c_str(), O_RDWR);
  if (fd < 0) {
    printf("failed to open calmcargpio\n");
    return;
  }

  int ret = 0;
  if (can_name_ == "can0") {
    ret = ::system(
        "echo 2 > /proc/irq/425/smp_affinity");  // redirection can0 irq to cpu1
    ret = ioctl(fd, 8, 0);
  }
  if (can_name_ == "can1") {
    //::system("echo 2 > /proc/irq/426/smp_affinity");  //redirection can1 irq
    // to cpu1
    ret = ioctl(fd, 9, 0);
  }
  if (can_name_ == "can2") {
    ret = ioctl(fd, 10, 0);
  }

  if (ret < 0) {
    printf("%s: failed to set can transciver on", can_name_.c_str());
  }
  close(fd);
}

bool SocketCAN::Init(bool gpio) {
  std::string cmd = "ifconfig " + can_name_ + " up";
  if (::system(cmd.c_str()) < 0) {
    printf("system exec error: %s\n", cmd.c_str());
  }
  socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  if (socket_ < 0) {
    printf("open can socket error\n");
    return false;
  }

  int canfd_on_ = 1;
  struct ifreq ifr_;
  memset(&ifr_, 0, sizeof(ifr_));
  memset(&ifr_.ifr_name, 0, sizeof(ifr_.ifr_name));
  strncpy(ifr_.ifr_name, can_name_.c_str(),
          strlen(can_name_.c_str()));                //网口名字
  ifr_.ifr_ifindex = if_nametoindex(ifr_.ifr_name);  //名字转为index
  if (ioctl(
          socket_, SIOCGIFINDEX,
          &ifr_)) {  //获取网卡接口，把接口的索引的存入ifr_ifindex，指定与can_name_绑定
    printf("ioctl error\n");
    return false;
  }

  // addr.can_ifindex = 0;
  addr_.can_family = AF_CAN;
  addr_.can_ifindex = ifr_.ifr_ifindex;
  can_err_mask_t err_mask = CAN_ERR_MASK;
  setsockopt(socket_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask,
             sizeof(err_mask));
  setsockopt(socket_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on_,
             sizeof(canfd_on_));
  // setsockopt(socket_,SOL_CAN_RAW,CAN_RAW_FILTER,NULL,0);

  if (bind(socket_, (struct sockaddr*)&addr_, sizeof(addr_)) < 0) {
    printf("bind can socket error\n");
  }
  char ctrl_msg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
  iov_.iov_base = &canfd_frame_;
  can_msg_hdr_.msg_name = &addr_;
  can_msg_hdr_.msg_iov = &iov_;
  can_msg_hdr_.msg_iovlen = 1;
  can_msg_hdr_.msg_control = ctrl_msg;
  if (gpio) {
    std::thread([this] { SetTransciverOn(); }).detach();
  }
  return true;
}

bool SocketCAN::Update() {
  fd_set can_set_;
  struct timeval wait_time;
  wait_time.tv_sec = 0;
  wait_time.tv_usec = 10 * 1000;  // 10ms
  if (socket_ < 0) {
    return false;
  }
  FD_ZERO(&can_set_);
  FD_SET(socket_, &can_set_);
  int ret = select(socket_ + 1, &can_set_, NULL, NULL, &wait_time);
  if (ret <= 0) {
    return false;
  }

  char ctrl_msg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
  iov_.iov_len = sizeof(canfd_frame_);
  can_msg_hdr_.msg_namelen = sizeof(addr_);
  can_msg_hdr_.msg_controllen = sizeof(ctrl_msg);
  can_msg_hdr_.msg_flags = 0;

  int nbytes = recvmsg(socket_, &can_msg_hdr_, MSG_DONTWAIT);
  if (nbytes < 0) return false;

  canid_t id = canfd_frame_.can_id & CAN_EFF_MASK;
  __u8 dlc = canfd_frame_.len;
  can_data.id = id;
  memcpy(can_data.data, &(canfd_frame_.data), dlc);
  can_data.dlc = dlc;
  can_data.channel = channel_id_;

  if (!absolute_timestamp_) {
    struct timeval tv;
    ioctl(socket_, SIOCGSTAMP, &tv);
    if (nbytes < 0 && errno == ENETDOWN) {
      return false;
    }
    can_data.timestamp = tv.tv_sec * 1000 * 1000 + tv.tv_usec;
  } else {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    can_data.timestamp = tv.tv_sec * 1000 * 1000 + tv.tv_usec;
  }

  return true;
}

bool SocketCAN::Write(const std::vector<RawCAN>& can_data) {
  for (size_t i = 0; i < can_data.size(); ++i) {
    bool sent = false;
    // retry for 10 times
    for (int k = 0; k < 10; ++k) {
      usleep(150);
      // yuping.li has confirmed that minimum delay between two can
      // frame is 150us
      if (WriteOne(can_data[i])) {
        sent = true;
        break;
      }
    }
    if (!sent) return false;
  }

  return true;
}

bool SocketCAN::WriteOne(const RawCAN& data) {
  int nbytes = 0;
  int size = 0;
  if (data.dlc <= CAN_MAX_DLEN) {
    struct can_frame frame = data.can_frame();
    nbytes = write(socket_, &frame, sizeof(frame));
    size = sizeof(frame);
  } else {
    struct canfd_frame frame = data.canfd_frame();
    nbytes = write(socket_, &frame, sizeof(frame));
    size = sizeof(frame);
  }
  if (nbytes != size && (errno == ENOBUFS)) {
    printf("send can data error: %d\n", errno);
    return false;
  }
  return true;
}

int SocketCAN::channel_id() const { return channel_id_; }

#endif  // not WIN32
